www.gusucode.com > gpsoft 的惯性导航工具箱源码程序 > matlab代做 修改 程序 gpsoft 的惯性导航工具箱/惯导工具箱/lclevupd.m

    function [DCM_ll_I, omega_el_L, omega_ie_L] = lclevupd(lat1,lat2,vx1,vx2,vy1,vy2,...
                  height1,height2,td12,tdex,tdint,DCMel,vertmech,procflg,earthflg)
%LCLEVUPD      Compute the direction cosine matrix relating local-level-frame 
%              motion (relative to the inertial frame) over an update interval.
%
%              Note that two effects are taken into account here.  One is the
%              motion of the vehicle relative to the earth (i.e., craft rate).
%              Second is the motion of the earth (and thus the local level
%              frame also) relative to the inertial frame
%
%	[DCM_ll_I,omega_el_L,omega_ie_L] = lclevupd(lat1,lat2,vx1,vx2,vy1,vy2,...
%           height1,height2,td12,tdex,tdint,DCMel,vertmech,procflg,earthflg)
%
%   INPUTS
%       NOTE: The '1' and '2' in the input arguments refer to time indices.
%             For example, lat1 is the latitude at time index 1 and lat2
%             is the latitude at time index 2.  The convention here is
%             that time index 1 is earlier than 2.  Usually, these indices
%             refer to the previous two position/velocity updates
%       latx = geodetic latitude in radians
%       vxx = local-level-frame x-coordinate velocity in m/s
%       vyx = local-level-frame y-coordinate velocity in m/s
%       heightx = vehicle height above the reference ellipsoid in meters
%       td12 = time difference (in seconds) between time indices 1 and 2
%              (this is a positive number; i.e., td12 = time2 - time1)
%       tdex = time difference between time index 2 and the median time
%              of the nav-frame update interval (i.e., extrapolated time point)
%              (this is a positive number;
%               i.e., tdex = extrapolated_time_point - time2)
%       tdint = navigation frame update interval (in seconds)
%       DCMel = 3x3 direction cosine matrix providing the
%             transformation from the earth frame
%             to the local-level (ENU) frame
%       vertmech = argument to specify vertical mechanization
%                  0 = north-pointing (default)
%                  1 = free azimuth (vertical craft rate set to zero)
%                  2 = foucault (spatial rate set to zero)
%                  3 = unipolar (wander angle rate set to +/- longitude
%                                rate; - for northern hemisphere;
%                                + for southern hemisphere)
%        procflg = processing flag; 0=first-order solution; 1=exact solution
%        earthflg = earth shape flag
%                   0 = spherical earth; 1 = WGS-84 ellipsoid (see CRAFRATE)
%
%   OUTPUTS
%       DCM_ll_I = the direction cosine matrix relating the local-level frame (ENU)
%             at the beginning of the update interval to the local-level frame
%             at the end of the update interval (relative to the inertial frame)
%       omega_el_L = craft rate (a.k.a., transport rate) vector; this is the
%             rotation rate of the local-level-frame relative to the earth frame
%             and it is expressed in local-level-frame coordinates (rad/sec)
%

%	M. & S. Braasch 4-98
%	Copyright (c) 1997-98 by GPSoft
%	All Rights Reserved.
%

if nargin<15,error('insufficient number of input arguments'),end
if (earthflg ~= 0) & (earthflg ~= 1), error('EARTHFLG not specified correctly'),end

C = [0 1 0; 1 0 0; 0 0 -1];    % conversion between NED and ENU

lat_ex = extrapol(lat1,lat2,td12,tdex);
vx_ex = extrapol(vx1,vx2,td12,tdex);
vy_ex = extrapol(vy1,vy2,td12,tdex);
height_ex = extrapol(height1,height2,td12,tdex);

omega_el_L = crafrate(lat_ex,vx_ex,vy_ex,height_ex,DCMel,earthflg,vertmech);
omega_ie_E = [0 0 7.292115e-5]';
omega_ie_L = DCMel*omega_ie_E;
omega_il_L = omega_ie_L + omega_el_L;

ang_vect = omega_il_L*tdint;
S = skewsymm(ang_vect);
if procflg == 0,
   DCM_ll_I = eye(3) - S;   % First order approximation
else
   magn = norm(ang_vect);
   if magn == 0,
      A = eye(3);
   else                   % Exact solution
      A = eye(3) - (sin(magn)/magn)*S + ( (1-cos(magn))/magn^2 )*S*S;
   end
   DCM_ll_I = A;
end